#include <stdlib.h>
#include "heap.h"

#define HEAP_PARENT(n) ((n - 1) >> 1)
#define HEAP_LEFT(n) ((n << 1) + 1)
#define HEAP_RIGHT(n) ((n << 1) + 2)

#define HEAP_EXCHANGE(HEAP,I1,I2) \
{ \
		void *aux = HEAP->array[I1]; \
		HEAP->array[I1] = HEAP->array[I2]; \
		heap->array[I2] = aux; \
}

/** \brief Testa se I1 them prioridade maior que I2
 */
#define HEAP_HIGHER(HEAP,I1,I2) (HEAP->func(HEAP->array[I1], HEAP->array[I2]) < 0)

/** Heap
 */
struct Heap_ {
	int alloc_size; /**< Tamanho alocado do heap */
	int num_el; /**< Número de elementos no heap */
	void **array; /**< Array que armazena os elementos */ 
	heapCmpFunc func; /**< Função de comparação do heap */
};

/* Constroi e inicializa um heap */
Heap heap_new (heapCmpFunc func)
{
	struct Heap_ *heap;

	heap = (struct Heap_ *) malloc (sizeof (struct Heap_));
	heap->array = (void **) malloc (128 * sizeof (void*));
	heap->alloc_size = 128;
	heap->num_el = 0;
	heap->func = func;
	return heap;
}

/* Novo elemento no heap */
void heap_push (Heap heap, void * el)
{
	int i;

	/* Aumenta o array se necessário */
	if (heap->num_el == heap-> alloc_size) {
		heap->alloc_size *= 2; 
		heap->array = (void **) realloc (heap->array, heap->alloc_size * sizeof (void*));
	}
	
	i = heap->num_el;
	heap->num_el++;
	heap->array[i] = el;
	/* Enquanto os filhos tem maior prioridade que os pais, os trocamos */
	while (i != 0 && HEAP_HIGHER(heap, i, HEAP_PARENT(i))) {
		HEAP_EXCHANGE (heap, i, HEAP_PARENT(i));
		i = HEAP_PARENT(i);
	}
}

/** \brief Move um elemento para baixo no heap até que ele esteja num ponto adequado
 *
 * \param heap Heap
 * \param i Índice do elemento
 */
static void heapfy (Heap heap, int i)
{
	int child; /* Nodo filho de maior prioridade */

	// Se o índice corresponde a uma folha, termina
	if (HEAP_LEFT(i) >= heap->num_el && HEAP_RIGHT(i) >= heap->num_el) {
		return;
	}
	// Se só há um filho, troca se necessário e termina
	if (HEAP_RIGHT(i) >= heap->num_el) { 
		if (HEAP_HIGHER (heap, HEAP_LEFT(i), i)) {
			HEAP_EXCHANGE(heap, i, HEAP_LEFT(i));
		}
		return;
	}
	// Há dois filhos

	// Encontramos o filho mais prioritário
	child = HEAP_HIGHER(heap, HEAP_LEFT(i), HEAP_RIGHT(i)) ? HEAP_LEFT(i) : HEAP_RIGHT(i);
	// Comparamos o filho mais prioritário com o pai
	if (HEAP_HIGHER (heap, child, i) ) {
		// Trocamos
		HEAP_EXCHANGE(heap, i, child);
		// Corrigimos a subárvore alterada
		return heapfy (heap, child);
	}
	// Já está no lugar certo. Terminamos
}

void *heap_pop (Heap heap)
{
	if (heap->num_el == 0) {
		return NULL;
	} else {
		void *top = heap->array[0];
		heap->num_el --;
		heap->array[0] = heap->array[heap->num_el];
		heapfy (heap, 0); // Corrige o heap
		return top;
	}
}


void heap_delete (Heap heap)
{
	free (heap->array);
	free(heap);
}

int heap_size (Heap heap)
{
	return heap->num_el;
}

void heap_forall (Heap heap, HeapForallFunc func, void *data)
{
	int i;
	
	for (i = 0; i < heap->num_el; i++)
		func (heap->array[i], data); 
}

void heap_remove (Heap heap, void *el)
{
	int i = 0;

	while (heap->array[i] != el && i < heap->num_el)
		i++;

	// Não encontrado
	if (i >= heap->num_el)
		return;

	// Se é o último, esquecemos dele
	if (i == heap->num_el - 1) {
		heap->num_el--;
		return;
	}
	
	// Está no meio do heap
	// Trocamos pelo último
	HEAP_EXCHANGE(heap, i, heap->num_el-1);
	// Reestruturamos o heap
	heap->num_el--;
	heapfy (heap, i);

}

